/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-08     86188       the first version
 */
#ifndef APPLICATIONS_INC_PID_H_
#define APPLICATIONS_INC_PID_H_


#include "stm32f4xx_hal.h"


#define PID_VEL_INTEGRATION_LIMIT  100.0
#define PID_VEL_OUTPUT_LIMIT       250.0 ///最终pwm限幅


#define PID_POSX_INTEGRATION_LIMIT  100.0
#define PID_POSX_OUTPUT_LIMIT       100.0 //期望速度限幅

#define PID_POSY_INTEGRATION_LIMIT  100.0
#define PID_POSY_OUTPUT_LIMIT       100.0

#define PID_POSZ_INTEGRATION_LIMIT  1000.0
#define PID_POSZ_OUTPUT_LIMIT       30.0


extern float VEL_KP;
extern float VEL_KI;
extern float VEL_KD;


extern float POSX_KP;
extern float POSX_KI;
extern float POSX_KD;

extern float POSY_KP;
extern float POSY_KI;
extern float POSY_KD;

extern float POSZ_KP;
extern float POSZ_KI;
extern float POSZ_KD;

typedef struct
{
    float desired;   //< set point
    float error;     //< error
    float prevError; //< previous error
    float integ;     //< integral
    float deriv;     //< derivative
    float kp;        //< proportional gain
    float ki;        //< integral gain
    float kd;        //< derivative gain
    float outP;      //< proportional output (debugging)
    float outI;      //< integral output (debugging)
    float outD;      //< derivative output (debugging)
    float iLimit;    //< integral limit
    float iLimitLow; //< integral limit
    float maxOutput;
    float dt; //< delta-time dt
    float out;
} PidObject;


extern PidObject leftFrontPid;
extern PidObject rightFrontPid;
extern PidObject leftBackPid;
extern PidObject rightBackPid;

extern PidObject carYawPid;

extern PidObject xPosPid;
extern PidObject yPosPid;
extern PidObject zPosPid;


void abs_outlimit(float *a, float ABS_MAX);
void pidSetPID(PidObject* pid, const float kp,const float ki,const float kd);
void pidReset(PidObject* pid);
void pidSetOutLimit(PidObject* pid, const float maxoutput) ;
void pidSetIntegralLimit(PidObject* pid, const float limit) ;
void pidUpdate(PidObject *pid,const float error);
void pidInit(void);

void velocityPidUpdata(PidObject* pid,float error);


#endif /* APPLICATIONS_INC_PID_H_ */
